/*
 Name:    motorControl.ino
 Created: 2021/1/6 17:27:39
 Author:  dlr123
*/

// 初始位置设定为
//前进方向是第一个物块抓取与放置的圆心连线上
//旋转方向是物块放置位置
//升降方向是最高点
//抓取方向是张开


//#include <MultiStepper.h>
#include <EEPROM.h>
#include <AccelStepper.h>
//#include <AccelStepper.cpp>
#define Mode_DRIVER 1
#define vertical_step 9 
#define rotate_step 6 
#define forward_step 5 
#define clutch_step 3 
#define vertical_dir 8   
#define forward_dir 4   
#define clutch_dir 2   
#define rotate_dir 7 
#define vertical_init 0
#define clutch_init 2
#define rotate_init 3  
#define forward_init 1   
#define all_init 4  
// the setup function runs once when you press reset or power the board
//选择一个电机以加减速运动在特定时间内运动到指定位置
void runStep(AccelStepper *step, long distanceToGoal, int runTime,int sub);
//清理串口缓存
void clearCache();
//判断是哪一种控制模式
void controlMethor(boolean num);
//手动控制时的控制函数
void humanControl(boolean num);
//自动工作时的控制函数
void automaticControl();
//位置初始化函数
void  initialPosition(int initNum);
//选择方向初始化
void initialRotate();
//前进方向初始化
void initialForward();
void initialClutch();
void initialVertical();
//前进方向的传感器中断
void finitialattach();
//旋转方向的传感器中断
void rinitialattach();
void cinitialattach();
void vinitialattach();
//指令控制时的命令函数集
void cmdstep(AccelStepper *stepper,char cmd,long data,byte n);
//以下是自动控制的八个工作流程
void task1(); // 回转到抓取处
void task2(); //下降爪子
void task3(); //收缩手爪
void task4(); //爪子上升+会转到放置处
void task5(); //下降爪子
void task6(); //松开手爪
void task7(); //上升爪子
void task8(byte times); //沿着导轨前进
void runUsrCmd(); //进入指令控制集合
void allQuickQuit(); //快速停止
void allQuit(); //减速停止
void stepperRun(int j); //在指令模式下驱动每个电机
void chooseRunmode(AccelStepper *stepper,int k); //在指令模式下，选择匀速运动还是加减速运动

long data; //参数
char cmd; //指令
int sub = 200; //细分
boolean num1 = 1,num2 = 1; //用以区分三种模式
boolean hm=0; //判断是否是指令模式
volatile boolean stepIsInitial[4]; //判断旋转方向是否归零
//volatile boolean rIsInitial = 0; //判断前进方向是否归零
boolean taskIsDown[7];  //在自动控制时，判断每一个任务是否完成
boolean work[7];  //在自动控制时，判断电机在每一个工作开始时是否初始化
boolean stepmode[4];  //判断特定电机在指令控制时是匀速还是加减速
byte times = 0; //在自动控制时判断完成了几轮循环工作

//定义四个电机
AccelStepper vertical(Mode_DRIVER, vertical_step, vertical_dir);
AccelStepper rotate(Mode_DRIVER, rotate_step, rotate_dir);
AccelStepper clutch(Mode_DRIVER, clutch_step, clutch_dir);
AccelStepper forward(Mode_DRIVER, forward_step, forward_dir);


void setup() {
  for (int j = 0; j < 4; j++)
  {
    stepIsInitial[j] = 0;
    stepmode[j] = 0;
  }
  for (int j = 0; j < 7; j++)
  {
    taskIsDown[j] = 0;
    work[j] = 0;
  }
  Serial.begin(9600);
  Serial.println("请输入控制模式数字");
  Serial.println("1 ： 手动控制    0：自动控制");
  while (! Serial.available()){}
  num1 = Serial.parseInt();
  clearCache();
  if (num1 == 1)
  {
    Serial.println("请选择指令控制还是摇杆控制:");
    Serial.println("1 ： 指令控制    0：摇杆控制");
    while (!Serial.available()) {}
    num2 = Serial.parseInt();
    clearCache();
    Serial.print("当前选择为:");
    if (num2 == 1)
    {
      Serial.println("指令模式");
    }
    else {
      Serial.println("遥感模式");
    }
  }
  else
  {
    Serial.print("当前选择为:");
    Serial.println("自动控制模式");
  }
  Serial.println("工作模式选择完毕,现在开始工作");

  //激活中断
  attachInterrupt(0, finitialattach, FALLING);
  attachInterrupt(1, rinitialattach, FALLING);
  attachInterrupt(2, vinitialattach, FALLING);
}
// the loop function runs over and over again until power down or reset
void loop() {
  if (Serial.available()) {     // 检查串口缓存是否有数据等待传输 
    cmd = Serial.read();        // 获取电机指令中指令信息    
    Serial.print(F("cmd = "));
    Serial.print(cmd);
    Serial.print(F(" , "));
    runUsrCmd();
    clearCache();
  }
  controlMethor(num1);
  // controlMethor()中设置了hm的值，当且仅当为指令模式的时候该逻辑生效
  if (hm) 
  {
    for (int j = 0; j < 4; j++)
    {
      stepperRun(j);
    }
  }
}
void runStep(AccelStepper *step, long distanceToGoal, int runTime,int sub)
{
  int a, maxspeed;
  distanceToGoal = distanceToGoal * sub/360;
  step->moveTo(distanceToGoal);
  a = step->distanceToGo() / pow(runTime / 2, 2);
  maxspeed = a * runTime / 2+sub/4;
  step->setAcceleration(a);
  step->setMaxSpeed(maxspeed);
}

void clearCache() {
  while (Serial.read() > 0);
}
void controlMethor(boolean num)
{
  if (num == 1) {
    if (num2 == 0) {
      humanControl(num2);
      hm = 0;
    }
    else
      hm = 1;
  }
  else {
    automaticControl();
    hm = 0;
  }
}

void automaticControl()
{
  if (!stepIsInitial[0] || !stepIsInitial[1] || !stepIsInitial[2] || !stepIsInitial[3]) {
    initialPosition(all_init);
    if (stepIsInitial[0] && stepIsInitial[1] && stepIsInitial[2] && stepIsInitial[3])
    {
      Serial.println("已经准备就绪，输入s即可开始工作");
      while (1)
      {
        if (Serial.available())
        {
          if (Serial.read() == 's')
          {
            clearCache();
            break;
          }
          else {
            clearCache();
          }
        }
      }
    }
  }   
  else
  {
    if (!taskIsDown[0])
    {
      task1();
    }
    else {
      if (!taskIsDown[1]) {
        task2();
      }
      else {
        if (!taskIsDown[2]) {
          task3();
        }
        else {
          if (!taskIsDown[3]) {
            task4();
          }
          else {
            if (!taskIsDown[4]) {
              task5();
            }
            else {
              if (!taskIsDown[5]) {
                task6();
              }
              else {
                if (!taskIsDown[6]) {
                  task7();
                }
                else {
                  times++;
                  if (times < 5) {

                    for (int i = 0; i < 7; i++)
                    {
                      taskIsDown[i] = !taskIsDown[i];
                      work[i] = !work[i];
                    }
                      task8(times);
                  }       
                }
              }
            }
          }
        }
      }
    }
  }
}
void humanControl(boolean num) {
  if (num)
  {
    delay(10);
    char s = Serial.read();
    Serial.print(F("s = "));
    Serial.print(s);
    Serial.println(F(""));

    data = Serial.parseInt();   // 获取电机指令中参数信息  
    Serial.print(F("data = "));
    Serial.print(data);
    Serial.println(F(""));
    switch (cmd)
    {
    case 'v':
      sub = 200;
      cmdstep(&vertical, s,data,0);
      break;
    case 'f':
      sub = 400;
      cmdstep(&forward, s, data,1);
      break;
    case 'c':
      sub = 24;
      cmdstep(&clutch, s, data,2);
      break;
    case 'r':
      sub = 6400;
      cmdstep(&rotate, s, data,3);
      break;
    }
  }
  else
  {

  }
}
void  initialPosition(int initNum)
{
  switch (initNum)
  {
  case all_init:
    initialForward();
    initialRotate();
    initialClutch();
    initialVertical();
    break;
  case rotate_init:
    stepIsInitial[3] = 1;
    initialRotate();
    break;
  case forward_init:
    stepIsInitial[1] = 1;
    initialForward();
    break;
  case clutch_init:
    stepIsInitial[2] = 1;
    initialClutch();
    break;
  case vertical_init:
    stepIsInitial[0] = 1;
    initialVertical();
    break;
  default:
    Serial.println(F("错误的初始化参数"));
    Serial.println(F("整体位置初始化请输入0"));
    Serial.println(F("摆臂位置初始化请输入1"));
    Serial.println(F("导轨位置初始化请输入2"));
    break;
  }
}
void initialForward()
{
  if (!stepIsInitial[1]) {
    forward.setMaxSpeed(1800);
    forward.setSpeed(900);
    forward.runSpeed();
  }
}
void finitialattach()
{
  forward.setCurrentPosition(0);
  stepIsInitial[1] = 1;
}


void initialRotate()
{
  if (!stepIsInitial[3]) {
    rotate.setMaxSpeed(6400);
    rotate.setSpeed(3200);
    rotate.runSpeed();
  }
}
void rinitialattach()
{
  rotate.setCurrentPosition(0);
  stepIsInitial[3] = 1;
}

void initialClutch() {
  if (!stepIsInitial[2])
  {

    if (clutch.distanceToGo() == 0)
    {
      stepIsInitial[2] = !stepIsInitial[2];
      clutch.setCurrentPosition(0);
    }
  }
}

void cinitialattach()
{

}

void initialVertical() {
  if (!stepIsInitial[0]) {
    vertical.setMaxSpeed(1000);
    vertical.setSpeed(800);
    vertical.runSpeed();
    }
  }
void vinitialattach()
{
  stepIsInitial[0] = 1;
  vertical.setCurrentPosition(0);
}



void task1()
{
  if (!work[0])
  {
    delay(100);
    runStep(&rotate, -180, 4, 6400);
    work[0] = !work[0];
  }
  else
  {
    rotate.run();
    forward.run();
    if (rotate.distanceToGo() == 0 && forward.distanceToGo() == 0)
    {
      taskIsDown[0] = !taskIsDown[0];
    }
  }
}
void task2()
{
  if (!work[1])
  {
    delay(100);
    runStep(&vertical, 900, 2, 400);
    work[1] = !work[1];
  }
  else
  {
    vertical.run();
    if (vertical.distanceToGo() == 0)
    {
      taskIsDown[1] = !taskIsDown[1];
    }
  }
}
void task3()
{
  if (!work[2])
  {
    delay(100);
    work[2] = !work[2];
    clutch.setMaxSpeed(120);
    clutch.setAcceleration(24);
    clutch.setSpeed(48);
    clutch.moveTo(0);
  }
  else
  {
    clutch.runSpeedToPosition();
    int N = analogRead(A0);
    if (N < 512)
    {
      clutch.setCurrentPosition(0);
      taskIsDown[2] = !taskIsDown[2];
    }
  }
}

void task4()
{
  if (!work[3])
  {
    delay(100);
    work[3] = !work[3];
    runStep(&rotate, 0, 4, 6400);
    runStep(&vertical, 0, 2, 400);
  }
  else {
    rotate.run();
    vertical.run();
    if (rotate.distanceToGo() == 0 && vertical.distanceToGo() == 0)
    {
      taskIsDown[3] = !taskIsDown[3];
    }
  }
}
void task5()
{
  if (!work[4])
  {
    delay(100);
    runStep(&vertical, 900, 2, 400);
    work[4] = !work[4];
  }
  else
  {
    vertical.runToPosition();
    if (vertical.distanceToGo() == 0)
    {
      taskIsDown[4] = !taskIsDown[4];
    }
  }
}
void task6()
{
  if (!work[5])
  {
    delay(100);
    runStep(&clutch, 240, 1, 24);
    work[5] = !work[5];
  }
  else
  {
    clutch.runToPosition();
    if (clutch.distanceToGo() == 0)
    {
      taskIsDown[5] = !taskIsDown[5];
    }
  }
}

void task7()
{
  if (!work[6])
  {
    delay(100);
    runStep(&vertical, 0, 2, 400);
    work[6] = !work[6];
  }
  else
  {
    vertical.runToPosition();
    if (vertical.distanceToGo() == 0)
    {
      taskIsDown[6] = !taskIsDown[6];
    }
  }
}
void task8(byte times)
{
  runStep(&forward, -3600 * times, 4, 400);
}

void runUsrCmd() {
  switch (cmd)
  {
  case 'q':
    data = Serial.parseInt();   // 获取电机指令中参数信息  
    Serial.print(F("data = "));
    Serial.print(data);
    Serial.println(F(""));
    if(data == 1)
      allQuickQuit();
    else
      allQuit();
    break;
  case 'm':
    data = Serial.parseInt();   // 获取电机指令中参数信息  
    Serial.print(F("data = "));
    Serial.print(data);
    Serial.println(F(""));
    if(data ==1 && num1==0){
      allQuit();
      num1 = data;
      Serial.println("请选择指令控制还是摇杆控制:");
      Serial.println("1 ： 指令控制    0：遥感控制");
      while (!Serial.available()) {}
      boolean n = Serial.parseInt();
        Serial.print("当前更换为:");
        if (n == 1)
        {
          Serial.println("指令模式");
        }
        else {
          Serial.println("遥感模式");
        }
        num2 = n;
    }
    else if (num1 == 1 && data == 1)
    {
      allQuit();
      Serial.println("请选择指令控制还是摇杆控制:");
      Serial.println("1 ： 指令控制    0：遥感控制");
      while (!Serial.available()) {}
      boolean n = Serial.parseInt();
      if (n != num2)
      {
        num2 = n;
        Serial.print("当前更换为:");
        if (n == 1)
        {
          Serial.println("指令模式");
        }
        else
        {
          Serial.println("遥感模式");
        }
      }
    }
    else if(data==0 && num1==1)
    {
      allQuit();
      num1 = data;
      Serial.print("当前更换为:");
      Serial.println("自动控制模式");
      for (int j; j < 4; j++)
      {
        stepIsInitial[j] = 0;
      }
    }
    break;
  default:
    if (hm)
    {
      humanControl(num2);
    }
    break;
  }
}
void allQuickQuit() {
  clutch.setCurrentPosition(clutch.currentPosition());
  forward.setCurrentPosition(forward.currentPosition());
  vertical.setCurrentPosition(vertical.currentPosition());
  rotate.setCurrentPosition(rotate.currentPosition());
}

void allQuit()
{
  clutch.setAcceleration(1000);
  forward.setAcceleration(5000);
  vertical.setAcceleration(5000);
  rotate.setAcceleration(40000);
  clutch.stop();
  forward.stop();
  vertical.stop();
  rotate.stop();
  while (clutch.isRunning() || forward.isRunning() || vertical.isRunning() || rotate.isRunning()) {
    clutch.run();
    forward.run();
    rotate.run();
    vertical.run();
  }

}

void cmdstep(AccelStepper* stepper, char cmd, long data,byte n)
{
  switch (cmd) {
  case 'o': //利用currentPosition获取当前电机输出轴位置并通过串口监视器输出该信息
    Serial.print(F("Motor Position: "));
    Serial.println((float)(stepper->currentPosition() * 360 / sub));
    break;

  case 'v': //利用moveTo函数使电机运行到用户指定坐标位置，moveTo函数不会“block”程序。

    Serial.print(F("Motor 'moveTo' "));  Serial.println((long)(data));
    stepper->moveTo((long)(data * sub / 360));
    break;

  case 'm':   // 利用move函数使电机运行相应步数。move函数不会“block”程序。

    Serial.print(F("Motor 'move'  "));  Serial.println((long)(data));
    stepper->move((long)(data * sub / 360));
    break;
  case 'r':  //利用runToNewPosition函数让电机运行到用户指定位置值。  
    Serial.print(F("Motor 'runToNewPosition' "));
    Serial.println((long)(data));
    stepper->runToNewPosition((long)(data * sub / 360));
    break;

  case 's':   //利用setCurrentPosition函数设置当前位置为用户指定位置值。
    Serial.print(F("Set Motor Current Position to "));
    Serial.println((long)(data * sub / 360));
    stepper->setCurrentPosition((long)(data * sub / 360));
    break;

  case 'a':  //利用setAcceleration函数设置加速度 
    Serial.print(F("Motor 'setAcceleration' "));
    Serial.println((float)(data));
    stepper->setAcceleration((float)(data * sub / 360));
    break;

  case 'x':  //利用setMaxSpeed函数设置最大速度   
    Serial.print(F("Motor 'setMaxSpeed' "));
    Serial.println((float)(data));
    stepper->setMaxSpeed((float)(data * sub / 360));
    break;

  case 'd':  //用户通过此指令可指定哪一个电机进行工作  
    if (data == 1)
      stepper->disableOutputs();
    else
      stepper->enableOutputs();
    break;
  case 'q':
    Serial.print(F("when stop,current speed:"));
    Serial.println((float)(stepper->speed() * 360 / sub));
    stepper->setAcceleration((float)(data * sub / 360));
    stepper->stop();
    float stopTime = millis();
    stepper->runToPosition();
    Serial.print(F("停止所花时间:"));
    Serial.println((millis() - stopTime) / 1000);
    break;
  case 'p':
    stepper->setCurrentPosition(stepper->currentPosition());
    Serial.println("已经强制停止");
    Serial.print("current speed:");
    Serial.println((float)(stepper->speed() * 360 / sub));
    break;
  case 'f':
    Serial.print("current speed:");
    Serial.println((float)(stepper->speed() * 360 / sub));
    break;
  case 'b':
    if (data > stepper->maxSpeed()) {
      Serial.println("超出最大速度，自动修改最大速度");
      stepper->setMaxSpeed((float)(data * sub / 360));
    }
    stepper->setSpeed((float)(data * sub / 360));
    Serial.print("set speed:");
    Serial.println((float)(stepper->speed() * 360 / sub));
  case 'n':
    stepmode[n] = data;
    if (data == 1)
    {
      Serial.println("切换为匀速模式");
    }
    if (data == 0)
    {
      Serial.println("切换为加减速模式");
    }
    break;
  default:  // 未知指令
    Serial.println(F("Unknown Command"));
  }
}

void stepperRun(int j)
{
  switch (j)
  {
  case 0:
    chooseRunmode(&vertical, j);
    break;
  case 1:
    chooseRunmode(&forward, j);
    break;
  case 2:
    chooseRunmode(&clutch, j);
    break;
  case 3:
    chooseRunmode(&rotate, j);
    break;
  }
}

void chooseRunmode(AccelStepper *stepper,int k) {
  if (stepmode[k]) //匀速
  {
    stepper->runSpeed();
  }
  else //加减速
  {
    stepper->run();
  }
}
